#include "stdafx.h"
#include "Boid.h"
#include "World.h"
//#include "ParamLoader.h"
#include "SteeringBehavior.h"

//------------------------------------------------------------------------
//	constructor
//
//------------------------------------------------------------------------
Boid::Boid(String entName,String meshName,SceneManager* sm,World* world, 
		   MyVec3 pos, MyReal radius,MyVec3 velocity,MyReal maxSpeed,MyVec3 heading, 
		   MyReal mass,MyRadian turnRate,MyReal maxForce)
	:mHeading(heading),
	mVelocity(velocity),
	mMass(mass),
	mSide(mHeading.perpendicular()),
	mMaxSpeed(maxSpeed),
	mMaxTurnRate(mMaxTurnRate),
	mMaxForce(maxForce),
	mWorld(world),
	mTimeElapsed(0.0)
{
	if(entName=="" || meshName=="")
		OGRE_EXCEPT(Ogre::Exception::ERR_INVALIDPARAMS,"","");
	mEntity = sm->createEntity(entName,meshName);
	sm->getRootSceneNode()->createChildSceneNode()->attachObject(mEntity);

	mSteering = new SteeringBehavior(this);
	mHeadingSmoother = new Smoother<MyVec3>(Prm.NumSamplesForSmoothing,MyVec3::ZERO);

	mAnimationState = getEntity()->getAnimationState("Idle");
	mAnimationState->setLoop(true);
	mAnimationState->setEnabled(true);
}

//------------------------------------------------------------------------
//	destructor
//	do not delete mEntity, this will be done by the Ogre's SceneManager
//------------------------------------------------------------------------
Boid::~Boid()
{ 
	mEntity=NULL;
	delete mSteering;
	delete mHeadingSmoother;
}

//--------------------------- RotateHeadingToFacePosition ---------------------
//
//  given a target position, this method rotates the entity's heading and
//  side vectors by an amount not greater than m_dMaxTurnRate until it
//  directly faces the target.
//
//  returns true when the heading is facing in the desired direction
//-----------------------------------------------------------------------------
bool Boid::rotateHeadingToFacePosition(OgreVec target)
{
	OgreVec toTarget = (target-getPos()).normalisedCopy();

	//first determine the angle between the heading vector and the target
	Ogre::Radian angle = Ogre::Radian(Ogre::Math::ACos(OgreVec(mHeading.ptr()).dotProduct(toTarget)));

	//return true if the player is facing the target
	if (angle.valueRadians() < 0.00001) return true;

	//clamp the amount to turn to the max turn rate
	if (angle > Ogre::Radian(mMaxTurnRate.valueRadians())) angle = Ogre::Radian(mMaxTurnRate.valueRadians());

	/*Matrix3 mat;
	mat.FromAxisAngle(Vector3(0,0,1),Radian(angle*mHeading.sign(toTarget)));
	mat.transformVector(mHeading);
	mat.transformVector(mVelocity);*/
	Ogre::Quaternion quat(angle,OgreVec::UNIT_Y);
	MyCommon::Quaternion myquat(quat.w,quat.x,quat.y,quat.z);

	mHeading = myquat*mHeading;
	mVelocity = myquat*mVelocity;
	getParentSceneNode()->setOrientation(quat);

	//finally recreate mSide
	mSide = mHeading.perpendicular();

	return false;
}

void Boid::setHeading(MyVec3 new_heading)
{
	assert((new_heading.squaredLength()-1.0)<0.00001);
	mHeading=new_heading;
	mSide = mHeading.perpendicular();
}

//------------------------------------------------------------------------
//	
//
//------------------------------------------------------------------------
void Boid::Update(double time_elapsed)
{
	mTimeElapsed=time_elapsed;

	//keep a record of its old position
	MyVec3 oldPos(getPos().ptr());

	MyVec3 steeringForce;

	steeringForce = mSteering->Calculate();

	//Acceleration = Force/Mass
	MyVec3 acceleration = steeringForce / mMass;

	//update velocity
	mVelocity += acceleration * time_elapsed; 

	//make sure Boid does not exceed maximum velocity
	mVelocity.truncate(mMaxSpeed);

	//update the position
	setPos(OgreVec((oldPos + mVelocity * time_elapsed).ptr()));

	if(mVelocity.squaredLength()>0.00000001)
	{
		mHeading = mVelocity.normalisedCopy();
		mSide = mHeading.perpendicular();
	}

	if (IsSmoothingOn())
	{
		mSmoothedHeading = mHeadingSmoother->update(getHeading());
	}

	mAnimationState->addTime(time_elapsed);
}